#!/usr/bin/python3
import threading
import json
import socket
import sys
import time
REMOTE_IP = "192.168.1.100"
LOCAL_IP = "192.168.1.125"


class driver:
    def __init__(self, local_ip) -> None:
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.sock.settimeout(1)
        self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
        self.sock.bind((local_ip, 8001))
        pass

    def send(self, cmd: dict):
        out = json.dumps(cmd)
        return self.sock.sendto(out.encode(), (REMOTE_IP, 8005))

    def recv(self):
        ret = self.sock.recv(1024)
        if (not ret):
            return None
        try:
            cmd = json.loads(ret)
            fb = {}
            fb["fl_pos"] = cmd["fl_pos"]
            fb["fl_vel"] = cmd["fl_vel"]

            fb["fr_pos"] = cmd["fr_pos"]
            fb["fr_vel"] = cmd["fr_vel"]

            fb["bl_pos"] = cmd["bl_pos"]
            fb["bl_vel"] = cmd["bl_vel"]

            fb["br_pos"] = cmd["br_pos"]
            fb["br_vel"] = cmd["br_vel"]
            return fb
        except:
            print("exception on load: ", ret)
            return None

    def printf(self, fb):
        print("Feedback Real:")
        print(" fl_pos ", fb["fl_pos"], " fl_vel ", fb["fl_vel"],
              " fr_pos ", fb["fr_pos"], " fr_vel ", fb["fr_vel"])
        print(" bl_pos ", fb["bl_pos"], " bl_vel ", fb["bl_vel"],
              " br_pos ", fb["br_pos"], " br_vel ", fb["br_vel"])
        print("-----------------------------------------------------------------------------------")


if __name__ != "__main__":
    print("should be run as main")
    exit()
if len(sys.argv) < 2:
    print("usage: %s <speed> <angle>" % (sys.argv[0]))
    exit()


dri = driver(LOCAL_IP)


def recv_thread():
    while True:
        try:
            rec = dri.recv()
            if (rec):
                dri.printf(rec)
        except:
            print("recv timeout")
            pass


th = threading.Thread(target=recv_thread, daemon=True)
th.start()  # detach
cmd = {}
cmd["fl_pos"] = int(sys.argv[2])
cmd["fl_vel"] = int(sys.argv[1])

cmd["fr_pos"] = int(sys.argv[2])
cmd["fr_vel"] = int(sys.argv[1])

cmd["bl_pos"] = int(sys.argv[2])
cmd["bl_vel"] = int(sys.argv[1])

cmd["br_pos"] = int(sys.argv[2])
cmd["br_vel"] = int(sys.argv[1])
while True:
    dri.send(cmd)
    # print(cmd)
    time.sleep(0.02)
